/**
 * @file Dispatcher.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 调度器类
 */

#include "Dispatcher.h"
#include "log.h"
#include "Map.h"
#include "Direction.h"
#include "utils.h"
#include <cmath>
#include "BFS.h"
#include "Path.h"
#include "Params.h"

/**
 * @brief 连接到机器人
 * @param robot 连接的机器人
 */
void Dispatcher::attach(Robot *robot) {
    this->robot = robot;
}

/**
 * @brief 工作循环
 */
void Dispatcher::loop() {
    int loopCnt = 1; // 循环次数
    while(loopCnt--) {
        switch(this->status) {
            default:
            case GET_CMD:
                if(!this->cmdSeq.empty()) {
                    this->cmd = this->cmdSeq.front();
                    this->status = START_CMD;
                    ++loopCnt;
                }
                break;

            case START_CMD:
                switch(this->cmd.type) {
                    case Cmd::RUN_TO:
                    { // 向后检查指令序列，找到下一个方向变化的指令，执行在此之前的指令
                        std::list<Cmd>::iterator last = this->cmdSeq.begin();
                        std::list<Cmd>::iterator it = last;
                        ++it;
                        while(it != this->cmdSeq.end() && it->type == Cmd::RUN_TO && it->data3 == last->data3) {
                            last = it;
                            ++it;
                        }
                        this->cmd = *last;
                        this->robot->runTo(Coord(this->cmd.data1, this->cmd.data2, MAP_SCALE), this->lastCmd.type != Cmd::RUN_TO);
                    }
                        break;
                    case Cmd::BUY:
                        if(this->robot->getBenchID() != reinterpret_cast<Bench *>(this->cmd.ptr)->getID()) { // 如果未抵达购买工作台，前往工作台
                            this->robot->runTo(*reinterpret_cast<Bench *>(this->cmd.ptr));
                        } else {
                            this->robot->stopCtrl();
                            if(reinterpret_cast<Bench *>(this->cmd.ptr)->getProductStatus()) { // 有物品时才能购买
                                this->robot->buy();
                            } else { // 向后检查指令序列，找到下一个方向变化的指令，转向这个指令的位置
                                std::list<Cmd>::iterator it = this->cmdSeq.begin();
                                while(it != this->cmdSeq.end() && it->type != Cmd::RUN_TO) { // 找到第一个抵达指令
                                    ++it;
                                }
                                if(it != this->cmdSeq.end()) {
                                    std::list<Cmd>::iterator last = it;
                                    ++it;
                                    while(it != this->cmdSeq.end() && it->type == Cmd::RUN_TO && it->data3 == last->data3) {
                                        last = it;
                                        ++it;
                                    }
                                    this->robot->turnTo(Coord(last->data1, last->data2, MAP_SCALE));
                                }
                            }
                        }
                        break;
                    case Cmd::SELL:
                        if(this->robot->getBenchID() != reinterpret_cast<Bench *>(this->cmd.ptr)->getID()) { // 如果未抵达出售工作台，前往工作台
                            this->robot->runTo(*reinterpret_cast<Bench *>(this->cmd.ptr));
                        } else {
                            this->robot->stopCtrl();
                            this->robot->sell();
                        }
                        break;
                    case Cmd::DESTROY:
                        this->robot->destroy();
                        break;
                    case Cmd::WAIT:
                    case Cmd::AVOID:
                        this->robot->stopCtrl();
                        { // 向后检查指令序列，找到下一个方向变化的指令，转向这个指令的位置
                            std::list<Cmd>::iterator it = this->cmdSeq.begin();
                            while(it != this->cmdSeq.end() && it->type != Cmd::RUN_TO) { // 找到第一个抵达指令
                                ++it;
                            }
                            if(it != this->cmdSeq.end()) {
                                std::list<Cmd>::iterator last = it;
                                ++it;
                                while(it != this->cmdSeq.end() && it->type == Cmd::RUN_TO && it->data3 == last->data3) {
                                    last = it;
                                    ++it;
                                }
                                this->robot->turnTo(Coord(last->data1, last->data2, MAP_SCALE));
                            }
                        }
                        break;
                    default:
                        break;
                }
                this->status = EXECUTE_CMD;
                ++loopCnt;
                break;

            case EXECUTE_CMD:
                switch(this->cmd.type) {
                    case Cmd::RUN_TO:
                        for(std::list<Cmd>::iterator it = this->cmdSeq.begin(); it != this->cmdSeq.end() && *it != this->cmd; ++it) {
                            if(this->robot->getCellX() == it->data1 && this->robot->getCellY() == it->data2) {
                                std::list<Cmd>::iterator now = it;
                                it = this->cmdSeq.begin();
                                while(it != now) { // 如果当前机器人坐标在指令序列中的目标指令前，弹出当前坐标之前的指令
                                    std::list<Cmd>::iterator del = it;
                                    ++it;
                                    this->cmdSeq.erase(del);
                                }
                                break;
                            }
                        }
                        if(this->robot->isFinishedCtrl()) { // 已经完成控制任务
                            int i = 0;
                            std::list<Cmd>::iterator it = this->cmdSeq.begin();
                            while(it != this->cmdSeq.end() && *it != this->cmd) { // 弹出终点前所有指令
                                std::list<Cmd>::iterator del = it;
                                ++it;
                                this->cmdSeq.erase(del);
                            }
                            this->robot->stopCtrl(); // 停止控制
                            this->status = END_CMD;
                            ++loopCnt;
                        }
                        break;
                    case Cmd::BUY:
                        if(!this->robot->isEmpty()) { // 已经买到物品
                            reinterpret_cast<Bench *>(this->cmd.ptr)->freeBuy();
                            this->status = END_CMD;
                            ++loopCnt;
                        } else {
                            this->status = START_CMD;
                        }
                        break;
                    case Cmd::SELL:
                        if(this->robot->isEmpty()) { // 已经没有携带物品
                            reinterpret_cast<Bench *>(this->cmd.ptr)->freeSell(this->cmd.data1);
                            this->status = END_CMD;
                            ++loopCnt;
                        } else {
                            this->status = START_CMD;
                        }
                        break;
                    case Cmd::DESTROY:
                        if(this->robot->isEmpty()) { // 已经没有携带物品
                            this->status = END_CMD;
                            ++loopCnt;
                        } else {
                            this->status = START_CMD;
                        }
                        break;
                    case Cmd::WAIT:
                        if(this->cmd.data1 <= 0) { // 等待时间已减为0
                            this->status = END_CMD;
                            ++loopCnt;
                        }
                        --this->cmd.data1;
                        break;
                    case Cmd::AVOID:
                        if(this->avoidDispatcher->isInPath(this->cmd.data1, this->cmd.data2, this->robot->isEmpty(), this->cmd.data3) == -1) { // 原冲突点已经不会再冲突
                            this->avoidDispatcher = nullptr;
                            this->status = END_CMD;
                            ++loopCnt;
                        }
                        break;
                    default:
                        break;
                }
                break;

            case END_CMD:
                this->lastCmd = this->cmd;
                this->cmd.type = Cmd::NOP;
                this->cmdSeq.pop_front();
                this->status = GET_CMD;
                ++loopCnt;
                break;

            case INSERT_CMD:
                this->status = GET_CMD;
                loopCnt = 1;
                break;
        }
    }
}

/**
 * @brief 添加指令
 * @param cmd 需要添加的指令
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd(Cmd cmd, bool isPushFront) {
    if(isPushFront) {
        this->status = INSERT_CMD;
        this->cmdSeq.push_front(cmd);
    } else {
        this->cmdSeq.push_back(cmd);
    }
}

/**
 * @brief 添加前往指令
 * @param x 前往格子的横坐标
 * @param y 前往格子的纵坐标
 * @param isEmpty 是否为空载
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd_RunTo(int x, int y, bool isEmpty, bool isPushFront) {
    this->pushCmd(Cmd::createRunTo(x, y, Direction::INVALID, isEmpty), isPushFront); // 方向设置为INVALID，执行时不会被压缩
}

/**
 * @brief 添加购买指令
 * @param bench 购买工作台
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd_Buy(Bench *bench, bool isPushFront) {
    this->pushCmd(Cmd::createBuy(bench), isPushFront);
}

/**
 * @brief 添加出售指令
 * @param bench 出售工作台
 * @param type 货物类型
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd_Sell(Bench *bench, int type, bool isPushFront) {
    this->pushCmd(Cmd::createSell(bench, type), isPushFront);
}

/**
 * @brief 添加销毁指令
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd_Destroy(bool isPushFront) {
    this->pushCmd(Cmd::createDestroy(), isPushFront);
}

/**
 * @brief 添加等待指令
 * @param waitFrames 等待帧数
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd_Wait(int waitFrames, bool isPushFront) {
    this->pushCmd(Cmd::createWait(waitFrames), isPushFront);
}

/**
 * @brief 添加避让指令
 * @param dispatcher 需要避让的机器人调度器
 * @param minSteps 最少检查指令数
 * @return true 存在避让路径
 * @return false 不存在避让路径
 */
bool Dispatcher::pushCmd_Avoid(Dispatcher *dispatcher, int minSteps) {
    int x = this->robot->getCellX();
    int y = this->robot->getCellY();
    if(dispatcher->isInPath(x, y, this->robot->isEmpty(), minSteps)) {
        std::list<Cmd> avoidCmdSeq = BFS::getAvoidCmdSeq(x, y, this->robot->isEmpty(), dispatcher, minSteps); // 躲避指令序列
        if(avoidCmdSeq.empty()) { // 如果躲避指令序列为空，说明不存在避让路径
            return false;
        }
        while(this->cmdSeq.front().type == Cmd::WAIT) { // 弹出所有等待指令
            this->cmdSeq.pop_front();
        }
        std::list<Cmd> backCmdSeq = avoidCmdSeq; // 返回指令序列
        backCmdSeq.pop_back(); // 最后一个点不需要走两次
        backCmdSeq.reverse();
        this->pushCmd(backCmdSeq, true); // 先前插返回队列
        std::list<Cmd>::iterator it = backCmdSeq.begin(); // 躲避序列的第一个点为最后在路径上的点
        this->pushCmd(Cmd::createAvoid(it->data1, it->data2, dispatcher, minSteps), true); // 前插躲避指令
        this->avoidDispatcher = dispatcher;
        this->pushCmd(avoidCmdSeq, true);
    }
    return true;
}

/**
 * @brief 添加指令序列
 * @param cmdSeq 需要添加的指令序列
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd(std::list<Cmd> cmdSeq, bool isPushFront) {
    if(cmdSeq.empty()) {
        return;
    }
    if(isPushFront) {
        this->status = INSERT_CMD;
        cmdSeq.splice(cmdSeq.end(), this->cmdSeq);
        this->cmdSeq.clear();
    }
    this->cmdSeq.splice(this->cmdSeq.end(), cmdSeq);
}

/**
 * @brief 添加购买到出售的指令序列
 * @param buy 购买工作台
 * @param sell 出售工作台
 * @param isPushFront 是否需要添加在序列最前方
 */
void Dispatcher::pushCmd(Bench *buy, Bench *sell, bool isPushFront) {
    this->pushCmd(Path::getCmdSeq(this->robot, buy, sell), isPushFront);
}

/**
 * @brief 清空指令
 */
void Dispatcher::clearCmd() {
    this->cmd = Cmd();
    this->status = Dispatcher::START_CMD;
    if(!this->robot->isEmpty()) {
        this->robot->destroy();
    }
    std::list<Cmd>::iterator it = this->cmdSeq.begin();
    while(it != this->cmdSeq.end()) {
        std::list<Cmd>::iterator del = this->cmdSeq.begin();
        if(it->type == Cmd::AVOID) {
            this->avoidDispatcher = nullptr;
        }
        if(it->type == Cmd::BUY) {
            reinterpret_cast<Bench *>(it->ptr)->freeBuy();
        }
        if(it->type == Cmd::SELL) {
            reinterpret_cast<Bench *>(it->ptr)->freeSell(it->data1);
        }
        ++it;
        this->cmdSeq.erase(del);
    }
}

/**
 * @brief 预测冲突
 * @param steps 预测的步数
 * @param anyDirection 不止考虑路径同向的机器人
 * @return int 发生冲突的机器人序号(-1为不冲突)
 */
int Dispatcher::forecastCrash(int steps, bool anyDirection) {
    int i = 0;
    for(std::list<Cmd>::iterator it = this->cmdSeq.begin(); it != this->cmdSeq.end(); ++it) {
        if(i++ >= steps) {
            return -1;
        }
        if(it->type == Cmd::AVOID) {
            return this->avoidDispatcher->robot->getID();
        }
        if(it->type != Cmd::RUN_TO) {
            --i;
            continue;
        }
        int isRobot = Map::isRobot(it->data1, it->data2, this->robot->getID(), this->robot->isEmpty(), anyDirection ? Direction::INVALID : it->data3);
        if(isRobot != -1) {
            return isRobot;
        }
    }
    return -1;
}

/**
 * @brief 获得调度器连接的机器人
 * @return Robot* 连接的机器人
 */
Robot *Dispatcher::getRobot() {
    return this->robot;
}

/**
 * @brief 获取调度器正在执行的指令
 * @return Cmd 正在执行的指令
 */
Cmd Dispatcher::getCmd() {
    return this->cmd;
}

/**
 * @brief 判断某点是否在调度器规划的下一个购买或出售点前的路径上
 * @param x 横坐标
 * @param y 纵坐标
 * @param isEmpty 是否使用空载状态的地图
 * @param minSteps 最小步数(如果到了出售点还没有达到步数，接着向后检查)
 * @return int 该点路径的方向(-1为不在路径上)
 */
int Dispatcher::isInPath(int x, int y, bool isEmpty, int minSteps) {
    int range = (this->robot->isEmpty() ? PARAMS.SAFETY_DISTANCE_EMPTY : PARAMS.SAFETY_DISTANCE_FULL) + (isEmpty ? PARAMS.SAFETY_DISTANCE_EMPTY : PARAMS.SAFETY_DISTANCE_FULL);
    if(utils::isInRectRange(x, y, this->robot->getCellX(), this->robot->getCellY(), range)) { // 该点在机器人上，方向无效
            return Direction::INVALID;
    }
    int steps = 0;
    bool needSearchMore = false;
    for(std::list<Cmd>::iterator it = this->cmdSeq.begin(); it != this->cmdSeq.end(); ++it) {
        ++steps;
        if(it->type != Cmd::RUN_TO) {
            if(it->type != Cmd::WAIT) {
                --steps; // 其余指令不算步数
            }
            if(steps < minSteps) {
                needSearchMore = true;
                continue;
            } else {
                return -1;
            }
        }
        if(needSearchMore && steps >= minSteps) {
            return -1;
        }
        if(utils::isInRectRange(x, y, it->data1, it->data2, range)) {
            return it->data3;
        }
    }
    return -1;
}

/**
 * @brief 判断是否正在规避指定机器人
 * @param avoidDispatcher 指定机器人的调度器
 * @return true 正在躲避
 * @return false 未在躲避
 */
bool Dispatcher::isAvoid(Dispatcher *avoidDispatcher) {
    if(avoidDispatcher) {
        return this->avoidDispatcher == avoidDispatcher;
    } else {
        return this->avoidDispatcher != nullptr;
    }
}

/**
 * @brief 是否正在等待
 * @return true 正在等待
 * @return false 未在等待
 */
bool Dispatcher::isWait() {
    return this->cmd.type == Cmd::WAIT;
}

/**
 * @brief 是否处于空闲状态
 * @return true 空闲
 * @return false 忙碌
 */
bool Dispatcher::isFree() {
    return this->cmdSeq.empty();
}
